#include <iostream>
#include <fstream>
#include <cmath>
#include <algorithm>
#include <sstream>
#include <string>
#include <chrono>
#include <assert.h>
#include <cmath>
// matplotlib
#include "matplotlibcpp.hpp"
#include "u_turn_planner.h"
#include "general_concepts.h"
#include "hybrid_a_star.h"
#include "utils.h"

using namespace std;

int main() {
    // load planner config from command line
    U_turn_scenario::MotionConfig motion_config = {
        1.0,    // desired_speed or a-star simulation speed
        3,      // ego car wheelbase
        40.0,   // max steering angle
        -40.0}; // min steering angle
    U_turn_scenario::hybrid_a_star::SearchConfig search_config = {
        40, // explore steering angle step
        1}; // time duration each exploration

    // load lane env from file
    U_turn_scenario::Lane start_lane;
    U_turn_scenario::Lane end_lane;
    start_lane.center_point = {{0.0, 100.0, M_PI / 2}};
    end_lane.center_point = {{8, 100.0, -M_PI / 2}};

    // init planner & do planning
    U_turn_scenario::UTurnPlanner u_turn_planner(motion_config);

    u_turn_planner.SetEnvironment(start_lane, end_lane);

    u_turn_planner.SetHybridAStarConfig(search_config);

    std::vector<U_turn_scenario::Point> a_star_trajectory; // x,y,theta,kappa

    std::chrono::system_clock::system_clock::time_point start_time =
        std::chrono::system_clock::system_clock::now();

    bool goal_reached = false;

    std::vector<U_turn_scenario::Point> trajectory = u_turn_planner.Plan(&a_star_trajectory, &goal_reached);

    std::chrono::system_clock::system_clock::time_point end_time =
                                                        std::chrono::system_clock::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end_time - start_time;
    std::cout << "time spent:" << elapsed_seconds.count() <<" s"<< std::endl;

    if (!goal_reached) {
        std::cout << "can not reach target goal, may be you need move a little bit" << std::endl;
    }

    // plot
    std::vector<double> x;
    std::vector<double> y;
    std::vector<double> theta;
    std::vector<double> u;
    std::vector<double> v;
    for (int i = 0; i < int(trajectory.size()); i++) {
        x.push_back(trajectory[i].x);
        y.push_back(trajectory[i].y);
        theta.push_back(trajectory[i].theta);
        u.push_back(std::cos(trajectory[i].theta));
        v.push_back(std::sin(trajectory[i].theta));
    }
    namespace plt = matplotlibcpp;
    plt::plot({x.front()}, {y.front()}, "r*"); //"ro"
    plt::plot({x.back()}, {y.back()}, "b*");   //"ro"
    plt::plot(x, y, "r*");                     //"ro"

    double l = 1;
    double w = 1.5;
    // for the start point
    std::vector<double> x_vertices;
    std::vector<double> y_vertices;
    // plot the vehicle
    for (int i = 0; i < int(x.size()); i++) {
        x_vertices.clear();
        y_vertices.clear();
        x_vertices.push_back(x[i] + l * cos(theta[i]) - w / 2 * sin(theta[i]));
        y_vertices.push_back(y[i] + l * sin(theta[i]) + w / 2 * cos(theta[i]));
        x_vertices.push_back(x[i] + l * cos(theta[i]) + w / 2 * sin(theta[i]));
        y_vertices.push_back(y[i] + l * sin(theta[i]) - w / 2 * cos(theta[i]));
        x_vertices.push_back(x[i] - l * cos(theta[i]) + w / 2 * sin(theta[i]));
        y_vertices.push_back(y[i] - l * sin(theta[i]) - w / 2 * cos(theta[i]));
        x_vertices.push_back(x[i] - l * cos(theta[i]) - w / 2 * sin(theta[i]));
        y_vertices.push_back(y[i] - l * sin(theta[i]) + w / 2 * cos(theta[i]));
        x_vertices.push_back(x[i] + l * cos(theta[i]) - w / 2 * sin(theta[i]));
        y_vertices.push_back(y[i] + l * sin(theta[i]) + w / 2 * cos(theta[i]));
        plt::plot(x_vertices, y_vertices, "r-");
    }
    std::map<std::string, std::string> config = {{"angles", "uv"}, {"color", "g"} /*,{"width","0.002"} */};
    plt::quiver(x, y, u, v, config);
    plt::show();
    return 0;
}
